#ifndef IMU_PROCESSOR_H
#define IMU_PROCESSOR_H 1

#include "mathtools.h"
//#include "imuconstants.h"


class IMUProcessor
{

  public:
    IMUProcessor(); 

    void setInitialValues(const Vector3D &a, const Vector3D &m, const Vector3D &g);

    void setMeasurementErrors(const Vector3D &aerr, const Vector3D &merr, const Vector3D &gerr);

    void process(const Vector3D &a, const Vector3D &m, const Vector3D &g, const int dt);

    void vectorToQuaternion(const Vector3D &a, const Vector3D &aerr, Quaternion &qpos, Quaternion &qposerr);

    void eliminateYaw(Quaternion &pos);

    void estimatePositionQuaternionNewton(Quaternion &angle, Quaternion &angleerr);


    // get raw position computed from acceleration only
    Quaternion getPosRawAcc();
    // get raw position computed from gyroonly
    Quaternion getPosRawGyr();
    // get position computed from combining acc and mag, found with the newton method
    Quaternion getPosNewton();
    // get respective position error
    Quaternion getPosRawAccError();
    // get respective position error
    Quaternion getPosRawGyrError();
    // get respective position error
    Quaternion getPosNewtonError();
    // get the full output of the Kalman filter (0-3 pos, 4-6 gyro bias)
    VectorND<7> getPosKalman();



  private:
    int id, prev_id;

    Quaternion posrawacc[2], posrawaccerr[2], posrawgyr[2], posrawgyrerr[2], posnwt[2], posnwterr[2];
    VectorND<7> pos[2];
    MatrixNxN<7> P[2];

    // error on measurable quantities. everybody square now!
    Vector3D m_aerr, m_merr, m_gerr;

    // reference magnetic field
    Vector3D rm;

    // current readings
    Vector3D a, m, g;
    double dt;





};

#endif

